#include "ros/ros.h"
#include "cv_bridge/cv_bridge.h"
#include <opencv2/opencv.hpp>
#include "image_transport/image_transport.h"
#include "inference_engine.hpp"
#include <samples/ocv_common.hpp>
#include <ngraph/ngraph.hpp>
#include <samples/slog.hpp>
#include <iostream>
#include "RosDetect.h"

int main(int argc, char **argv)
{
    

    // //初始化节点
    ros::init(argc, argv, "object_detect");
    // RosDetect det;
    // ros::spin();

    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);

    //定义节点发布器
    image_transport::Publisher pub_raw_image = it.advertise("/classifier/image_cam", 1);     //原图
    // image_transport::Publisher pub_colored_image = it.advertise("/classifier/image_colored", 1);  //语义图
    // image_transport::Publisher pub_depth_image = it.advertise("/classifier/image_depth_copy", 1); //深度图

    cv::VideoCapture cap(0); //加载视频
    cv::Mat image;

    ros::Rate loop_rate(100); //设置发送频率

    while (nh.ok())
    {
        cap >> image;

        cv::Mat image_colored(image.size(), CV_8UC3, cv::Scalar(128, 128, 128));
        std_msgs::Header header;
        header.stamp = (ros::Time)0;
        header.frame_id = "0";

        // sensor_msgs::ImagePtr colored_msg = cv_bridge::CvImage(header, "bgr8", image_colored).toImageMsg();
        sensor_msgs::ImagePtr raw_msg = cv_bridge::CvImage(header, "bgr8", image).toImageMsg(); //将Mat类型数据转换成ROS的sensor_msg类型
        pub_raw_image.publish(raw_msg);
        // pub_colored_image.publish(colored_msg); //发布topic
        loop_rate.sleep();
    }
}
